/*

radio.pde -> Input, Output, Devices

ArduPilotMegaJeti Version 1.0 Public Beta
Author:     Uwe Gartmann

Authors of ArduPilotMega:	Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Michael Smith
Thanks to:	Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi

This firmware is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
/// Definitions


// Radio channels
// Note channels are from 0!  
#define CH_ROLL       0
#define CH_PITCH      1
#define CH_THROTTLE   2
#define CH_RUDDER     3
#define CH_FLIGHTMODE 7
#define CONTROLMODE_DELAY 25	// 25 x 20ms = 500ms

/// Includes

/// Declarations
uint8_t currControlPos = 0;
uint8_t prevControlPos = 0;
uint8_t tmpControlPos = 0;
uint8_t cntrControlPos = 0;	// prevent accidently switching to other modes while turning the knob

uint8_t reverse_roll = 1;
uint8_t reverse_pitch = 1;
uint8_t reverse_rudder = 1;
uint8_t delta_mix = 0;

char* control_mode_names[] = {	
	"undefined",
	"DIRECT",
	"MANUAL",
	"STABILIZE",
	"FBW_A",
	"FBW_B",
	"CIRCLE",
	"LOITER",
	"RTL",
	"TAKEOFF",
	"AUTO",
	"LAND"};

void initRC(void) {
	RC.Init();
	read_reverse_switches();
}

void updateRC(void) {
	RC.InputAllCh();
	updateControlSwitch();
}

void read_reverse_switches() {
	// all switches up means no reverse
	delta_mix = (PINL & 128) ? 0 : 1;
	//if (mix_mode == 0) {
		// up is reverse
		reverse_roll   = (PINE & 128) ? 1 : -1;
		reverse_pitch  = (PINE & 64) ? 1 : -1;
		reverse_rudder = (PINL & 64) ? 1 : -1;
	//} else {
		// up is elevon
		//reverse_elevons 	= (PINE & 128) ? 1 : -1;
		//reverse_ch1_elevon 	= (PINE & 64) ? 1 : -1;
		//reverse_ch2_elevon 	= (PINL & 64) ? 1 : -1;
	//}
}

void updateReverseSwitches(void) {
	read_reverse_switches();
}

void updateControlSwitch(void) {
	
	uint8_t tmpPos;
	
	// read actual position
	uint16_t pulsewidth = RC.CH_In[FLIGHT_MODE_CHANNEL - 1];
	
	// due to the function of the MUX, CH8 pulses longer than 1900us 
	// are threated as failsafe and are send directly to servos.
	
	// check pulsewidth and set mode
	if (pulsewidth > FLIGHT_MODE_1_BOUNDARY) {
		tmpPos = 1; 
	} else if (pulsewidth > FLIGHT_MODE_2_BOUNDARY) {
		tmpPos = 2;
	} else if (pulsewidth > FLIGHT_MODE_3_BOUNDARY) {
		tmpPos = 3;
	} else if (pulsewidth > FLIGHT_MODE_4_BOUNDARY) {
		tmpPos = 4;
	} else if (pulsewidth > FLIGHT_MODE_5_BOUNDARY) {
		tmpPos = 5;
	} else tmpPos = 6;
	
	if (tmpPos != currControlPos) {							// has mode changed?	
		if (tmpPos != tmpControlPos) {						// mode changed during delay?
			cntrControlPos = 0;								// reset counter
			tmpControlPos = tmpPos;							// save mode for next check
		} else {												// same mode as previous check
			if (!((currControlPos == 0) && (tmpPos != 1))) {	// mode has never been set, mode 1 is forced
				cntrControlPos++;								// increment delay counter
			}
		}
		if (cntrControlPos > CONTROLMODE_DELAY) {				// delay done
			prevControlPos = currControlPos;					// save previous mode
			currControlPos = tmpControlPos;					// set new mode
			cntrControlPos = 0;								// reset delay counter
		}
	} else {
		cntrControlPos = 0;									// mode has not changed, reset delay counter
	}
}

uint8_t getControlPos(void) {
	return currControlPos;
}

uint8_t getPrevControlPos(void) {
	return prevControlPos;
}

